#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
#include <fastdds/dds/domain/DomainParticipant.hpp>
#include <fastdds/dds/publisher/DataWriter.hpp>
#include <fastdds/dds/publisher/Publisher.hpp>

#include <unistd.h>
#include "HelloWorldPubSubTypes.h"

using namespace eprosima::fastdds::dds;

class HelloWorldPublisher
{
public:
    HelloWorldPublisher()
    :   type_(new HelloWorldPubSubType())
    {

    }

    ~HelloWorldPublisher()
    {
        if (writer_)
        {
            publisher_->delete_datawriter(writer_);
        }
        if (publisher_)
        {
            participant_->delete_publisher(publisher_);
        }
        if (topic_)
        {
            participant_->delete_topic(topic_);
        }

        DomainParticipantFactory::get_instance()->delete_participant(participant_);
    }

    bool init()
    {
        hello_.index(0);
        hello_.message("HelloWorld");

        DomainParticipantQos participantQos;
        participantQos.name("Participant_publisher");
        participant_ = DomainParticipantFactory::get_instance()->create_participant(0, participantQos);
        if (participant_ == nullptr)
        {
            return false;
        }

        type_.register_type(participant_);

        topic_ = participant_->create_topic("HelloWorldTopic", "HelloWorld", TOPIC_QOS_DEFAULT);
        if (topic_ == nullptr)
        {
            std::cout << "create_topic() failed!" << std::endl;
            return false;
        }

        publisher_ = participant_->create_publisher(PUBLISHER_QOS_DEFAULT);
        if (publisher_ == nullptr)
        {
            std::cout << "create_publisher() falied!" << std::endl;
            return false;
        }

        writer_ = publisher_->create_datawriter(topic_, DATAWRITER_QOS_DEFAULT);
        if (writer_ == nullptr)
        {
            std::cout << "create_datawriter() failed!" << std::endl;
            return false;
        }
        
        auto ret = participant_->enable();
        if (ret != eprosima::fastrtps::types::ReturnCode_t::RETCODE_OK)
        {
            std::cout << "Failed" << std::endl;
            return false;
        }

        return true;
    }

    bool publish()
    {
        hello_.index(hello_.index() + 1);
        if (writer_->write(&hello_))
        {
            std::cout << "write() succeed" << std::endl;
        }
        else
        {
            std::cout << "write() failed" << std::endl;
        }
        return true;
    }

    void run()
    {
        while (true)
        {
            publish();
            sleep(1);
        }
    }

private:
    DomainParticipant *participant_;
    TypeSupport type_;
    Topic *topic_;
    Publisher *publisher_;
    DataWriter *writer_;
    HelloWorld hello_;
};

int main()
{
    HelloWorldPublisher *mypub = new HelloWorldPublisher();
    if (!mypub->init())
    {
        std::cout << "init() failed!" << std::endl;
        return -1;
    }
    mypub->run();
    
    delete mypub;
    return 0;
}